home *** CD-ROM | disk | FTP | other *** search
/ GFX Sensations 1 / Graphic Sensations - Volume 1.iso / tools / amiga / 3d_tools / irit40s.lha / Irit / poly3d-h / out-edge.c < prev    next >
Encoding:
C/C++ Source or Header  |  1993-12-30  |  17.8 KB  |  519 lines
  1. /*****************************************************************************
  2. *   Routines to    test all edges for visibility relative to the global polygon *
  3. * list,    and output the visible ones, to    graphics screen    or as text file         *
  4. *                                         *
  5. * Written by:  Gershon Elber                Ver 2.0, Jan. 1989   *
  6. *****************************************************************************/
  7.  
  8. #include <stdio.h>
  9. #include <math.h>
  10. #include <setjmp.h>
  11. #include <ctype.h>
  12. #include <string.h>
  13. #include "program.h"
  14. #include "genmat.h"
  15.  
  16. #define    MAX_POLYLINE_SIZE    50     /* Maximum size of output polyline. */
  17. #define EDGE_ON_PLANE_EPS     -0.003  /* Epsilon considers edge on plane. */
  18.  
  19. static EdgeStruct *VisOutEdgeHashTable[EDGE_HASH_TABLE_SIZE];
  20. static EdgeStruct *HidOutEdgeHashTable[EDGE_HASH_TABLE_SIZE];
  21.  
  22. static void SaveCurrentMatrix(FILE *OutFile);
  23. static void OutputEdge(EdgeStruct *PEtemp, EdgeStruct *OutEdgeHashTable[]);
  24. static void EmitOutEdgeHashTable(FILE *OutFile,
  25.                  EdgeStruct *OutEdgeHashTable[]);
  26. static char *Real2Str(RealType R);
  27. static EdgeStruct *FoundMatchEdge(int *Colinear, EdgeStruct *PEdge,
  28.                   EdgeStruct *OutEdgeHashTable[]);
  29. static int FuzzSamePoints(RealType Pt1[3], RealType Pt2[3]);
  30. static int ColinearPoints(RealType Pt1[3], RealType Pt2[3], RealType Pt3[3]);
  31. static int VisibleEdge(int EdgeMinY, EdgeStruct *PEdge,
  32.                     IPPolygonStruct *PolyHashTbl[]);
  33. static int VisibleEdgeOnePoly(EdgeStruct *PEdge, IPPolygonStruct *PPoly);
  34. static int InverseMatrix(MatrixType A);
  35.  
  36. /*****************************************************************************
  37. * Routine to test for visibility all edges in EdgeHashTable and    display    or   *
  38. * output the visible ones only.    It is assumed that only    totally    visible    or   *
  39. * invisible edges are in table (Pass 3 broke all other kind of edges).         *
  40. *****************************************************************************/
  41. void OutVisibleEdges(FILE *OutFile)
  42. {
  43.     int    i;
  44.     EdgeStruct *PEtemp, *PEtail;
  45.  
  46.     IritCPUTime(TRUE);
  47.  
  48.     if (!GlblQuiet)
  49.     fprintf(stderr, "\nPass 4, Edges [%5d] =      ", EdgeCount);
  50.     for (i = 0; i < EDGE_HASH_TABLE_SIZE; i++)
  51.     VisOutEdgeHashTable[i] = HidOutEdgeHashTable[i] = NULL;
  52.  
  53.     if (!InverseMatrix(GlblViewMat)) {
  54.     fprintf(stderr,
  55.             "\nNo inverse for matrix transformation, output is in screen space\n");
  56.     MatGenUnitMat(GlblViewMat);         /* No inverse transform at all. */
  57.     }
  58.  
  59.     IRIT_DATA_HEADER(OutFile, "Poly3d-h");
  60.  
  61.     EdgeCount =    0;
  62.  
  63.     /* Output the viewing matrices. */
  64.     SaveCurrentMatrix(OutFile);
  65.  
  66.     for (i = 0; i < EDGE_HASH_TABLE_SIZE; i++)
  67.     if ((PEtemp = EdgeHashTable[i]) != NULL)/* If any edge in that list. */
  68.         while (PEtemp) {
  69.         EdgeCount++;
  70.         if (!GlblQuiet)
  71.             fprintf(stderr, "\b\b\b\b\b%5d", EdgeCount);
  72.         PEtail = PEtemp    -> Pnext;       /* OutputEdge destroy it. */
  73.         if ((!PEtemp -> Internal || GlblInternal))
  74.             if (VisibleEdge(i, PEtemp, PolyHashTable))
  75.             OutputEdge(PEtemp, VisOutEdgeHashTable);
  76.             else if (GlblOutputHiddenData)
  77.             OutputEdge(PEtemp, HidOutEdgeHashTable);
  78.         PEtemp = PEtail;
  79.         }
  80.  
  81.  
  82.     /* Output the visible data: */
  83.     if (GlblOutputHasRGB) {
  84.     fprintf(OutFile, "[OBJECT [COLOR %d] [RGB %d,%d,%d] [WIDTH %8.6lf] Visible\n",
  85.         GlblOutputColor,
  86.         GlblOutputRGB[0], GlblOutputRGB[1], GlblOutputRGB[2],
  87.         GlblOutputWidth);
  88.     }
  89.     else {
  90.     fprintf(OutFile, "[OBJECT [COLOR %d] [WIDTH %8.6lf] Visible\n",
  91.         GlblOutputColor, GlblOutputWidth);
  92.     }
  93.     /* Output all visible data. */
  94.     EmitOutEdgeHashTable(OutFile, VisOutEdgeHashTable);
  95.     fprintf(OutFile, "]\n\n");
  96.  
  97.     /* Output the hidden data if requested: */
  98.     if (GlblOutputHiddenData) {
  99.     if (GlblOutputHasRGB) {
  100.         fprintf(OutFile, "[OBJECT [HIDDEN] [COLOR %d] [RGB %d,%d,%d] [WIDTH %8.6lf] Hidden\n",
  101.             HIDDEN_COLOR,
  102.             GlblOutputRGB[0] / HIDDEN_COLOR_RATIO,
  103.             GlblOutputRGB[1] / HIDDEN_COLOR_RATIO,
  104.             GlblOutputRGB[2] / HIDDEN_COLOR_RATIO,
  105.             GlblOutputWidth / HIDDEN_WIDTH_RATIO);
  106.     }
  107.     else {
  108.         fprintf(OutFile, "[OBJECT [HIDDEN] [COLOR %d] [WIDTH %8.6lf] Hidden\n",
  109.             HIDDEN_COLOR, GlblOutputWidth / HIDDEN_WIDTH_RATIO);
  110.     }
  111.     /* Output all hidden data. */
  112.     EmitOutEdgeHashTable(OutFile, HidOutEdgeHashTable);
  113.     fprintf(OutFile, "]\n\n");
  114.     }
  115.  
  116.     if (!GlblQuiet)
  117.     fprintf(stderr, ",  %6.2lf seconds.", IritCPUTime(FALSE));
  118. }
  119.  
  120. /*****************************************************************************
  121. * Routine to save current view trans. IritPrsrViewMat to a generic mat file  *
  122. *****************************************************************************/
  123. static void SaveCurrentMatrix(FILE *OutFile)
  124. {
  125.     int    i, j;
  126.  
  127.     fprintf(OutFile, "[OBJECT MATRICES\n    [OBJECT VIEW_MAT\n\t[MATRIX");
  128.     for (i = 0; i < 4; i++) {
  129.     fprintf(OutFile, "\n\t    ");
  130.     for (j = 0; j < 4; j++)
  131.         fprintf(OutFile, "%12lf ", IritPrsrViewMat[i][j]);
  132.     }
  133.     fprintf(OutFile, "\n\t]\n    ]\n");
  134.  
  135.     if (IritPrsrWasPrspMat) {
  136.     fprintf(OutFile, "    [OBJECT PRSP_MAT\n\t[MATRIX");
  137.     for (i = 0; i < 4; i++) {
  138.         fprintf(OutFile, "\n\t    ");
  139.         for (j = 0; j < 4; j++)
  140.         fprintf(OutFile, "%12lf ", IritPrsrPrspMat[i][j]);
  141.     }
  142.     fprintf(OutFile, "\n\t]\n    ]\n");
  143.     }
  144.  
  145.     fprintf(OutFile, "]\n\n");
  146. }
  147.  
  148. /*****************************************************************************
  149. * Routine to output one    edge to    the OutEdgeHashTable.                 *
  150. * The edge is inserted into OutEdgeHashTable to    be able    to match it into     *
  151. * the longest path possible - connecting edges into edge sequence.         *
  152. * Each edge is inserted    by its Ymin, which will    cause the paths    be in         *
  153. * increased Y order...                                 *
  154. *****************************************************************************/
  155. static void OutputEdge(EdgeStruct *PEtemp, EdgeStruct *OutEdgeHashTable[])
  156. {
  157.     int    Level;
  158.  
  159.     Level = (int) ((PEtemp -> Vertex[0]    -> Coord[1] + 1.0) *
  160.                             EDGE_HASH_TABLE_SIZE2);
  161.     Level = BOUND(Level, 0, EDGE_HASH_TABLE_SIZE1);
  162.     PEtemp -> Pnext = OutEdgeHashTable[Level];
  163.     OutEdgeHashTable[Level] = PEtemp;
  164. }
  165.  
  166. /*****************************************************************************
  167. * Routine to scan the OutEdgeHashTable,    and Emit the longest paths found in  *
  168. * it by    searching for consecutive edges    and combining colinear edges.         *
  169. *****************************************************************************/
  170. static void EmitOutEdgeHashTable(FILE *OutFile, EdgeStruct *OutEdgeHashTable[])
  171. {
  172.     int    i, j, Colinear, Count;
  173.     RealType Vec[MAX_POLYLINE_SIZE][3];
  174.     EdgeStruct *PEtemp, *PEnext;
  175.  
  176.     for    (i = 0; i < EDGE_HASH_TABLE_SIZE; i++)     /* Scan all the hash table. */
  177.     while (OutEdgeHashTable[i]) {         /* Scan all edges in entry. */
  178.         PEtemp = OutEdgeHashTable[i];      /* Take edge out of table. */
  179.         OutEdgeHashTable[i]    = OutEdgeHashTable[i] -> Pnext;
  180.  
  181.         /* Print first vertices of polyline: */
  182.         Count = 0;
  183.         MatMultVecby4by4(Vec[Count++], PEtemp -> Vertex[0] -> Coord,
  184.                                 GlblViewMat);
  185.         MatMultVecby4by4(Vec[Count++], PEtemp -> Vertex[1] -> Coord,
  186.                                 GlblViewMat);
  187.  
  188.         while ((PEnext = FoundMatchEdge(&Colinear, PEtemp,
  189.                         OutEdgeHashTable)) != NULL) {
  190.         if (Colinear)                /* Overwrite last entry. */
  191.             MatMultVecby4by4(Vec[Count - 1],
  192.                      PEnext -> Vertex[1] -> Coord,
  193.                      GlblViewMat);
  194.         else
  195.             MatMultVecby4by4(Vec[Count++],
  196.                      PEnext -> Vertex[1] -> Coord,
  197.                      GlblViewMat);
  198.         if (Count >= MAX_POLYLINE_SIZE)
  199.             break;
  200.         PEtemp = PEnext;
  201.         }
  202.  
  203.         fprintf(OutFile, "    [POLYLINE %d\n", Count);
  204.         for (j = 0; j < Count; j++)
  205.         fprintf(OutFile, "\t[%s %s %s]\n",
  206.             Real2Str(Vec[j][0]),
  207.             Real2Str(Vec[j][1]),
  208.             Real2Str(Vec[j][2]));
  209.  
  210.         fprintf(OutFile, "    ]\n");
  211.     }
  212. }
  213.  
  214. /*****************************************************************************
  215. * Convert a real number into a string.                         *
  216. * The routine mentains 3 different buffers simultanuously so up to 3 calls   *
  217. * can be issued from same printf...                         *
  218. *****************************************************************************/
  219. static char *Real2Str(RealType R)
  220. {
  221.     static int i = 0;
  222.     static char Buffer[3][LINE_LEN_LONG];
  223.     int j, k;
  224.  
  225.     if (ABS(R) < 1e-6)
  226.     R = 0.0;            /* Round off very small numbers. */
  227.  
  228.     sprintf(Buffer[i], "%-8.6lg", R);
  229.  
  230.     for (k = 0; !isdigit(Buffer[i][k]) && k < LINE_LEN_LONG; k++);
  231.     if (k >= LINE_LEN_LONG) {
  232.     fprintf(stderr, "Conversion of real number failed.\n");
  233.     Poly3dhExit(3);
  234.     }
  235.  
  236.     for (j = strlen(Buffer[i]) - 1; Buffer[i][j] == ' ' && j > k; j--);
  237.     if (strchr(Buffer[i], '.') != NULL)
  238.     for (; Buffer[i][j] == '0' && j > k; j--);
  239.     Buffer[i][j+1] = 0;
  240.  
  241.     j = i;
  242.     i = (i + 1) % 3;
  243.     return Buffer[j];
  244. }
  245.  
  246. /*****************************************************************************
  247. * Routine to scan the OutEdgeHashTable for a match edge    if any,    delete it    *
  248. * from HashTable and return it.    if colinear with PEdge set Colinear to TRUE. *
  249. * return FALSE if no match found (end of polyline).                 *
  250. *****************************************************************************/
  251. static EdgeStruct *FoundMatchEdge(int *Colinear, EdgeStruct *PEdge,
  252.                   EdgeStruct *OutEdgeHashTable[])
  253. {
  254.     int    Level;
  255.     EdgeStruct *PEtemp, *PElast;
  256.  
  257.     Level = (int) ((PEdge -> Vertex[1] -> Coord[1] + 1.0) *
  258.                             EDGE_HASH_TABLE_SIZE2);
  259.     Level = BOUND(Level, 0, EDGE_HASH_TABLE_SIZE1);
  260.     PEtemp = PElast = OutEdgeHashTable[Level];
  261.     while (PEtemp) {                /* First scan for colinear edge. */
  262.     if (FuzzSamePoints(PEdge -> Vertex[1] -> Coord,
  263.                PEtemp -> Vertex[0] -> Coord) &&
  264.         ColinearPoints(PEdge -> Vertex[0] -> Coord,
  265.                PEdge -> Vertex[1] -> Coord,
  266.                PEtemp -> Vertex[1] -> Coord)) {
  267.         *Colinear =    TRUE;
  268.         /* Delete that edge    from hash table    structure: */
  269.         if (PEtemp == PElast)
  270.         OutEdgeHashTable[Level] = OutEdgeHashTable[Level] -> Pnext;
  271.         else
  272.         PElast -> Pnext = PEtemp -> Pnext;
  273.  
  274.         if (FuzzSamePoints(PEtemp -> Vertex[0] -> Coord,
  275.                    PEtemp -> Vertex[1] -> Coord))
  276.         return FoundMatchEdge(Colinear, PEdge, OutEdgeHashTable);
  277.         else
  278.             return PEtemp;
  279.     }
  280.     PElast = PEtemp;
  281.     PEtemp = PEtemp    -> Pnext;
  282.     }
  283.  
  284.     PEtemp = PElast = OutEdgeHashTable[Level];
  285.     while (PEtemp) {                /* Next scan for any match edge. */
  286.     if (FuzzSamePoints(PEdge -> Vertex[1] -> Coord,
  287.                PEtemp -> Vertex[0] -> Coord)) {
  288.         *Colinear =    FALSE;
  289.         /* Delete that edge    from hash table    structure: */
  290.         if (PEtemp == PElast)
  291.         OutEdgeHashTable[Level] = OutEdgeHashTable[Level] -> Pnext;
  292.         else
  293.             PElast -> Pnext = PEtemp -> Pnext;
  294.  
  295.         if (FuzzSamePoints(PEtemp -> Vertex[0] -> Coord,
  296.                    PEtemp -> Vertex[1] -> Coord))
  297.         return FoundMatchEdge(Colinear, PEdge, OutEdgeHashTable);
  298.         else
  299.         return PEtemp;
  300.     }
  301.     PElast = PEtemp;
  302.     PEtemp = PEtemp    -> Pnext;
  303.     }
  304.  
  305.     return NULL;                  /* No match - end of polyline. */
  306. }
  307.  
  308. /*****************************************************************************
  309. * Routine to test if two points    are almost the same, and returns TRUE if so. *
  310. *****************************************************************************/
  311. static int FuzzSamePoints(RealType Pt1[3], RealType Pt2[3])
  312. {
  313.     return (APX_EQ(Pt1[0], Pt2[0]) &&
  314.         APX_EQ(Pt1[1], Pt2[1]) &&
  315.         APX_EQ(Pt1[2], Pt2[2]));
  316. }
  317.  
  318. /*****************************************************************************
  319. * Routine to test if three points are colinear,    and returns TRUE if so...    *
  320. * Algorithm: cross product should be zero if colinear...             *
  321. * Note this routine does not return TRUE if Pt2    is not between Pt1..Pt3.     *
  322. *****************************************************************************/
  323. static int ColinearPoints(RealType Pt1[3], RealType Pt2[3], RealType Pt3[3])
  324. {
  325.     int    i;
  326.     RealType Xout, Yout, Zout, U[3], V[3], temp;
  327.  
  328.     for    (i = 0; i < 3; i++) {
  329.     U[i] = Pt2[i] -    Pt1[i];
  330.     V[i] = Pt3[i] -    Pt2[i];
  331.     }
  332.     temp = sqrt(SQR(U[0]) + SQR(U[1]) + SQR(U[2]));
  333.     if (APX_EQ(temp, 0.0))
  334.     return TRUE;
  335.     for    (i = 0; i < 3; i++)
  336.     U[i] = U[i] / temp;
  337.  
  338.     temp = sqrt(SQR(V[0]) + SQR(V[1]) + SQR(V[2]));
  339.     if (APX_EQ(temp, 0.0))
  340.     return TRUE;
  341.     for    (i = 0; i < 3; i++)
  342.     V[i] = V[i] / temp;
  343.  
  344.     /* Xoutput = Uy * Vz - Uz *    Vy. */
  345.     Xout = U[1]    /* Uy */  * V[2] /* Vz */  -
  346.        U[2]    /* Uz */  * V[1] /* Vy */;
  347.  
  348.     /* Youtput = Uz * Vx - Ux *    Vz. */
  349.     Yout = U[2]    /* Uz */  * V[0] /* Vx */  -
  350.        U[0]    /* Ux */  * V[2] /* Vz */;
  351.  
  352.     /* Zoutput = Ux * Vy - Uy *    Vx. */
  353.     Zout = U[0]    /* Ux */  * V[1] /* Vy */  -
  354.        U[1]    /* Uy */  * V[0] /* Vx */;
  355.  
  356.     return APX_EQ(Xout, 0.0) && APX_EQ(Yout, 0.0) && APX_EQ(Zout, 0.0) &&
  357.        ((MIN(Pt1[0], Pt3[0]) < Pt2[0]) && (MAX(Pt1[0], Pt3[0]) > Pt2[0]) &&
  358.         (MIN(Pt1[1], Pt3[1]) < Pt2[1]) && (MAX(Pt1[1], Pt3[1]) > Pt2[1]));
  359. }
  360.  
  361. /*****************************************************************************
  362. * Routine to test the visibility of the    given edge relative to all polygons  *
  363. * in polygon list. return TRUE if the edge is visible. It is assumed that    *
  364. * the edge is whole visible or whole invisible (Pass 3 broke the edge if     *
  365. * that whas not    true). Also it is assumed the polygons are all convex.         *
  366. * A short cut is made to test the edge only against the    needed polygons         *
  367. * only, using the polygon hash table and Y level sorting.             *
  368. *****************************************************************************/
  369. static int VisibleEdge(int EdgeMinY, EdgeStruct * PEdge,
  370.                           IPPolygonStruct *PolyHashTbl[])
  371. {
  372.     int    EdgeMaxY, i;
  373.     IPPolygonStruct *PPolyList, *PPolyLast;
  374.  
  375.     EdgeMaxY = (int) (MIN((MAX(PEdge -> Vertex[0] -> Coord[1],
  376.                    PEdge -> Vertex[1] -> Coord[1]) + 1.0) *
  377.                         EDGE_HASH_TABLE_SIZE2,
  378.               EDGE_HASH_TABLE_SIZE1));
  379.  
  380.     /* Test the    edge only in the interval 0..EdgeMaxY as these are the only  */
  381.     /* which might hide    that edge. Also    polygons with MaxY less    than         */
  382.     /* EdgeMinY    are deleted from PolyHashTbl as it is assumed the edges      */
  383.     /* are scanned in increasing order of the EdgeHashTable.             */
  384.     for    (i = 0; i <= EdgeMaxY; i++) {
  385.     PPolyList = PPolyLast =    PolyHashTbl[i];
  386.     while (PPolyList) {
  387.         if ((PPolyList -> BBox[1][1] + 1.0) *
  388.                           EDGE_HASH_TABLE_SIZE2 < EdgeMinY)
  389.         /* Delete this polygon!    */
  390.         if (PPolyList == PPolyLast) {
  391.             PolyHashTbl[i] = PPolyLast = PPolyList -> Pnext;
  392.             PPolyList =    PPolyList -> Pnext;
  393.         }
  394.         else {
  395.             PPolyList =    PPolyList -> Pnext;
  396.             PPolyLast -> Pnext = PPolyList;
  397.         }
  398.         else {         /* Polygon still active - test edge against it. */
  399.         /* If found one    polygon    that hides this    edge return FALSE... */
  400.  
  401.         if (!VisibleEdgeOnePoly(PEdge, PPolyList))
  402.             return FALSE;
  403.         PPolyLast = PPolyList;
  404.         PPolyList = PPolyList -> Pnext;
  405.         }
  406.     }
  407.     }
  408.  
  409.     return TRUE;
  410. }
  411.  
  412. /*****************************************************************************
  413. * Routine to test the visibility of the    given edge relative to one polygon   *
  414. * return TRUE if the edge is visible. It is assumed that the edge is whole   *
  415. * visible or whole invisible (Pass 3 broke the edge if that was not true).   *
  416. * Also it is assumed the polygons are all convex.                 *
  417. *****************************************************************************/
  418. static int VisibleEdgeOnePoly(EdgeStruct *PEdge, IPPolygonStruct *PPoly)
  419. {
  420.     int    i;
  421.     RealType MidPt[3];
  422.     IPVertexStruct *PList;
  423.  
  424.     for    (i = 0; i < 3; i++) MidPt[i] =        /* Calc a mid point on the edge: */
  425.         (PEdge -> Vertex[0] -> Coord[i] +
  426.          PEdge -> Vertex[1] -> Coord[i]) / 2.0;
  427.  
  428.     /* Test if edges is    out of polygon boundary    box: */
  429.     if (MidPt[0] > PPoly -> BBox[1][0] || MidPt[0] < PPoly -> BBox[0][0] ||
  430.     MidPt[1] > PPoly -> BBox[1][1] || MidPt[1] < PPoly -> BBox[0][1])
  431.     return TRUE;
  432.  
  433.     if (PPoly -> Plane[0] * MidPt[0] +
  434.     PPoly -> Plane[1] * MidPt[1] +
  435.     PPoly -> Plane[2] * MidPt[2] +
  436.     PPoly -> Plane[3] > EDGE_ON_PLANE_EPS)
  437.     return TRUE;                /* Edge in front of polygon. */
  438.  
  439.     /* We cannt    escape it - we now must    find if    the point is included in */
  440.     /* polygon area. We    assume the polygon is convex and use the fact     */
  441.     /* that the    polygon    list is    ordered    such that the cross product     */
  442.     /* of two consecutive vertices and the point is positive for all     */
  443.     /* consecutive vertices pairs iff the point    is in the polygon.     */
  444.     PList = PPoly -> PVertex;
  445.     while (PList -> Pnext) {
  446.     if (CrossProd(PList -> Coord, PList -> Pnext -> Coord, MidPt) < 0)
  447.         return TRUE;                  /* Out of polygon! */
  448.     PList =    PList -> Pnext;
  449.     }
  450.     /* Now test    last polygon edge (last    point, first point in poly list) */
  451.     if (CrossProd(PList -> Coord, PPoly -> PVertex -> Coord, MidPt) < 0)
  452.     return TRUE;                      /* Out of polygon! */
  453.  
  454.     return FALSE;
  455. }
  456.  
  457. /*****************************************************************************
  458. * Procedure to calculate the INVERSE of    a given    MatrixType matrix A which is *
  459. * modified to the inverted matrix. Return TRUE if inverse exists.         *
  460. *****************************************************************************/
  461. static int InverseMatrix(MatrixType A)
  462. {
  463.     MatrixType InvA;
  464.     int    i, j, k;
  465.     RealType V, D;
  466.  
  467.     MatGenUnitMat(InvA);
  468.  
  469.     for    (i = 0; i < 4; i++) {
  470.     V = A[i][i];                      /* Find the new pivot. */
  471.     k = i;
  472.     for (j = i + 1; j < 4; j++)
  473.         if (A[j][i] > V) {
  474.         /* Find maximum on col i, row i+1..4. */
  475.         V = A[j][i];
  476.         k = j;
  477.         }
  478.  
  479.     j = k;
  480.  
  481.     if (i != j)
  482.         for (k = 0; k < 4; k++) {
  483.         /* Swap. */
  484.         D = A[i][k]; A[i][k] = A[j][k]; A[j][k] = D;
  485.         D = InvA[i][k]; InvA[i][k] = InvA[j][k]; InvA[j][k] = D;
  486.         }
  487.  
  488.     for (j = i + 1; j < 4; j++) {    /* Eliminate col i from row i+1..4. */
  489.         V =    A[j][i]    / A[i][i];
  490.         for    (k = 0; k < 4; k++) {
  491.         A[j][k]       -= V    * A[i][k];
  492.         InvA[j][k] -= V    * InvA[i][k];
  493.         }
  494.     }
  495.     }
  496.  
  497.     for    (i = 4 - 1; i >= 0; i--) {               /* Back Substitution. */
  498.     if (A[i][i] == 0)
  499.         return FALSE;                       /* Error. */
  500.  
  501.     for (j = 0; j < i; j++) {     /* Eliminate col i from row 1..i-1. */
  502.         V =    A[j][i]    / A[i][i];
  503.         for    (k = 0; k < 4; k++) {
  504.         /* A ->    [j][k] -= V * A[i][k]; */
  505.         InvA[j][k] -= V    * InvA[i][k];
  506.         }
  507.     }
  508.     }
  509.  
  510.     for    (i = 0; i < 4; i++)            /* Normalize the inverse Matrix. */
  511.     for (j = 0; j < 4; j++)
  512.         InvA[i][j] /= A[i][i];
  513.     for    (i = 0; i < 4; i++)               /* Copy inverted    matrix to A. */
  514.     for (j = 0; j < 4; j++)
  515.         A[i][j] = InvA[i][j];
  516.  
  517.     return TRUE;
  518. }
  519.